#include "M_usart.h"
#include "usart.h"
#include "jy61p.h"

uint8_t USART3_DMA_Buffer[JY61P_PACKET_SIZE * 3]; // JY61P DMA接收缓冲区
uint8_t USART1_RxData[200];
uint8_t USART2_RxData[200];

uint8_t USART3_RxData;
uint8_t USART4_RxData[200];
uint8_t USART5_RxData[200];
// 使用示例
char lie[3] = {0};	// 初始化为0
char hang[3] = {0}; // 初始化为0
uint8_t row;		// 行
uint8_t col;
uint8_t dis_counter = 0;
// 串口发送相关变量
uint8_t uart_tx_buffer[128];	   // 发送缓冲区
volatile uint8_t uart_tx_busy = 0; // 发送忙标志
uint8_t last_row = 0;
uint8_t last_col = 0;

extern DMA_HandleTypeDef hdma_uart4_rx;
extern DMA_HandleTypeDef hdma_uart4_tx;
extern DMA_HandleTypeDef hdma_uart5_rx;
extern DMA_HandleTypeDef hdma_uart5_tx;
extern DMA_HandleTypeDef hdma_usart1_rx;
extern DMA_HandleTypeDef hdma_usart2_rx;

extern DMA_HandleTypeDef hdma_usart3_rx;
NoFlyZone_t nfz_data[3];
uint8_t nfz_received_count = 0;

// 简单禁飞区数组
SimpleNoFlyZone_t simple_nfz[3];
uint8_t simple_nfz_count = 0;
uint8_t nfz_sorted = 0; // 排序完成标志位：0=未排序，1=已排序
// 解析网格格式的动物数据函数（格式：animal,种类数量,列坐标,行坐标,@）
static int parse_animal_grid_data(const char *data, int *species_count, char *grid_position)
{
	const char *ptr = data;

	// 检查开头是否为"animal,"
	if (strncmp(ptr, "animal,", 7) != 0)
	{
		return 0; // 格式错误
	}
	ptr += 7; // 跳过"animal,"

	// 解析种类数量（整数）
	*species_count = 0;
	while (*ptr >= '0' && *ptr <= '9')
	{
		*species_count = *species_count * 10 + (*ptr - '0');
		ptr++;
	}
	if (*ptr != ',')
		return 0; // 格式错误
	ptr++;		  // 跳过逗号

	// 解析列坐标（如A3）
	if (*ptr < 'A' || *ptr > 'I') // 检查A-I范围
		return 0;
	char col_char = *ptr;
	ptr++;

	// 解析列号（1-9）
	int col_num = 0;
	while (*ptr >= '0' && *ptr <= '9')
	{
		col_num = col_num * 10 + (*ptr - '0');
		ptr++;
	}
	if (col_num < 1 || col_num > 9) // 检查1-9范围
		return 0;

	if (*ptr != ',')
		return 0; // 格式错误
	ptr++;		  // 跳过逗号

	// 解析行坐标（如B1）
	if (*ptr != 'B') // 必须是B
		return 0;
	ptr++;

	// 解析行号（1-7）
	int row_num = 0;
	while (*ptr >= '0' && *ptr <= '9')
	{
		row_num = row_num * 10 + (*ptr - '0');
		ptr++;
	}
	if (row_num < 1 || row_num > 7) // 检查1-7范围
		return 0;

	// 检查结尾是否为",@"
	if (strncmp(ptr, ",@", 2) != 0)
	{
		return 0; // 格式错误
	}

	// 组装网格位置字符串（如"A3B1"）
	sprintf(grid_position, "%c%d%c%d", col_char, col_num, 'B', row_num);

	return 1; // 解析成功
}

// 快速解析动物数据函数（避免使用sscanf提升性能）
static int parse_animal_data(const char *data, int *species_count, float *x, float *y)
{
	const char *ptr = data;

	// 检查开头是否为"animal,"
	if (strncmp(ptr, "animal,", 7) != 0)
	{
		return 0; // 格式错误
	}
	ptr += 7; // 跳过"animal,"

	// 解析种类数量（整数）
	*species_count = 0;
	while (*ptr >= '0' && *ptr <= '9')
	{
		*species_count = *species_count * 10 + (*ptr - '0');
		ptr++;
	}
	if (*ptr != ',')
		return 0; // 格式错误
	ptr++;		  // 跳过逗号

	// 解析x坐标（浮点数，支持负数）
	*x = 0.0f;
	int x_negative = 0;
	if (*ptr == '-')
	{
		x_negative = 1;
		ptr++; // 跳过负号
	}
	float decimal = 0.1f;
	int has_decimal = 0;
	while ((*ptr >= '0' && *ptr <= '9') || *ptr == '.')
	{
		if (*ptr == '.')
		{
			has_decimal = 1;
		}
		else if (!has_decimal)
		{
			*x = *x * 10.0f + (*ptr - '0');
		}
		else
		{
			*x += (*ptr - '0') * decimal;
			decimal *= 0.1f;
		}
		ptr++;
	}
	if (x_negative)
		*x = -*x; // 应用负号
	if (*ptr != ',')
		return 0; // 格式错误
	ptr++;		  // 跳过逗号

	// 解析y坐标（浮点数，支持负数）
	*y = 0.0f;
	int y_negative = 0;
	if (*ptr == '-')
	{
		y_negative = 1;
		ptr++; // 跳过负号
	}
	decimal = 0.1f;
	has_decimal = 0;
	while ((*ptr >= '0' && *ptr <= '9') || *ptr == '.')
	{
		if (*ptr == '.')
		{
			has_decimal = 1;
		}
		else if (!has_decimal)
		{
			*y = *y * 10.0f + (*ptr - '0');
		}
		else
		{
			*y += (*ptr - '0') * decimal;
			decimal *= 0.1f;
		}
		ptr++;
	}
	if (y_negative)
		*y = -*y; // 应用负号

	// 检查结尾是否为",@"
	if (strncmp(ptr, ",@", 2) != 0)
	{
		return 0; // 格式错误
	}

	return 1; // 解析成功
}

// 飞机坐标转网格坐标函数
// 飞机坐标系：右下角(A9B1)为原点，向左为X负，向上为Y正
// 地图：450cm×350cm，9×7网格，每格50cm×50cm
// 验证参考点：A8B1(-50,0), A9B2(0,50), A1B7(-400,300)
static int pixel_to_grid(float x, float y, char *grid_str)
{
	// 坐标转换公式（已验证）：
	// 飞机X坐标 → 网格A坐标：x=-400→A1, x=-50→A8, x=0→A9
	int grid_a = 9 + (int)(x / 50.0f);
	// 飞机Y坐标 → 网格B坐标：y=0→B1, y=50→B2, y=300→B7
	int grid_b = (int)(y / 50.0f) + 1;

	// 边界检查：超出范围直接抛弃
	if (grid_a < 1 || grid_a > 9 || grid_b < 1 || grid_b > 7)
	{
		return 0; // 超出范围
	}

	// 格式化为字符串
	sprintf(grid_str, "A%dB%d", grid_a, grid_b);
	return 1; // 成功
}

// 网格坐标转飞机坐标函数
// 网格坐标 → 飞机坐标系统（以A9B1为原点）
static void grid_to_aircraft_coords(uint8_t grid_a, uint8_t grid_b, float *x, float *y)
{
	// 转换公式：
	// 飞机X = (9 - grid_a) * 50  // A9→0, A8→50, A1→400
	// 飞机Y = (grid_b - 1) * 50  // B1→0, B2→50, B7→300
	*x = (9 - grid_a) * 50.0f;
	*y = (grid_b - 1) * 50.0f;
}

drone_data_t drone_data;		  // 初始化无人机数据
AnimalData animal_data = {0, ""}; // 初始化动物数据
uint8_t new_data_flag = 0;		  // 新数据标志位（全局变量）

/**
 * @brief 添加禁飞区（仅存储，不排序）
 * @param row 行号 (0-6)
 * @param col 列号 (0-8)
 */
void AddNoFlyZone(uint8_t row, uint8_t col)
{
	// 如果已经有3个禁飞区，不再添加
	if (simple_nfz_count >= 3)
	{
		return;
	}

	// 只存储，不排序
	simple_nfz[simple_nfz_count].row = row;
	simple_nfz[simple_nfz_count].col = col;
	simple_nfz[simple_nfz_count].num = simple_nfz_count + 1; // 临时序号
	simple_nfz[simple_nfz_count].type = 2;					 // 初始化为其他类型
	simple_nfz_count++;

	// 添加新数据后，标记为未排序
	nfz_sorted = 0;
}

/**
 * @brief 对禁飞区进行排序并分配正确序号
 * @note 在主循环中调用，不在中断中调用
 * SortNoFlyZones()// 使用禁飞区数据前，确保数据已排序
if (!nfz_sorted) {
	SortNoFlyZones();  // 如果未排序，先排序
}

// 现在可以安全使用 simple_nfz[] 数组中的数据
for (uint8_t i = 0; i < simple_nfz_count; i++) {
	// 使用 simple_nfz[i].row, simple_nfz[i].col, simple_nfz[i].num// 使用前确保已排序
if (!nfz_sorted) {
	SortNoFlyZones();
}

// 判断禁飞区类型
if (simple_nfz[0].type == 0) {
	// row相同的禁飞区（横向排列）
} else if (simple_nfz[0].type == 1) {
	// col相同的禁飞区（纵向排列）
} else {
	// 其他类型（不规则排列）
}
}
 */
void SortNoFlyZones(void)
{
	// 如果已经排序过或者数据不足，直接返回
	if (nfz_sorted || simple_nfz_count <= 1)
	{
		return;
	}

	// 检查是否为同一列（col相同）
	uint8_t same_col = 1;
	for (uint8_t i = 1; i < simple_nfz_count; i++)
	{
		if (simple_nfz[i].col != simple_nfz[0].col)
		{
			same_col = 0;
			break;
		}
	}

	// 检查是否为同一行（row相同）
	uint8_t same_row = 1;
	for (uint8_t i = 1; i < simple_nfz_count; i++)
	{
		if (simple_nfz[i].row != simple_nfz[0].row)
		{
			same_row = 0;
			break;
		}
	}

	if (same_col)
	{
		// col相同，按row从小到大排序
		for (uint8_t i = 0; i < simple_nfz_count - 1; i++)
		{
			for (uint8_t j = i + 1; j < simple_nfz_count; j++)
			{
				if (simple_nfz[i].row > simple_nfz[j].row)
				{
					// 交换
					SimpleNoFlyZone_t temp = simple_nfz[i];
					simple_nfz[i] = simple_nfz[j];
					simple_nfz[j] = temp;
				}
			}
		}
		// 设置类型为col相同
		for (uint8_t i = 0; i < simple_nfz_count; i++)
		{
			simple_nfz[i].type = 1; // col相同
		}
	}
	else if (same_row)
	{
		// row相同，按col从小到大排序
		for (uint8_t i = 0; i < simple_nfz_count - 1; i++)
		{
			for (uint8_t j = i + 1; j < simple_nfz_count; j++)
			{
				if (simple_nfz[i].col > simple_nfz[j].col)
				{
					// 交换
					SimpleNoFlyZone_t temp = simple_nfz[i];
					simple_nfz[i] = simple_nfz[j];
					simple_nfz[j] = temp;
				}
			}
		}
		// 设置类型为row相同
		for (uint8_t i = 0; i < simple_nfz_count; i++)
		{
			simple_nfz[i].type = 0; // row相同
		}
	}
	else
	{
		// 既不是同行也不是同列，保持type=2（其他类型）
		for (uint8_t i = 0; i < simple_nfz_count; i++)
		{
			simple_nfz[i].type = 2; // 其他类型
		}
	}

	// 重新分配序号1,2,3
	for (uint8_t i = 0; i < simple_nfz_count; i++)
	{
		simple_nfz[i].num = i + 1;
	}

	// 标记为已排序
	nfz_sorted = 1;
}

void Uart_Init(void)
{
	// 初始化UART4 DMA接收
	HAL_UARTEx_ReceiveToIdle_DMA(&huart4, USART4_RxData, sizeof(USART4_RxData));
	__HAL_DMA_DISABLE_IT(&hdma_uart4_rx, DMA_IT_HT);

	// 初始化UART5 DMA接收
	HAL_UARTEx_ReceiveToIdle_DMA(&huart5, USART5_RxData, sizeof(USART5_RxData));
	__HAL_DMA_DISABLE_IT(&hdma_uart5_rx, DMA_IT_HT);

	// 初始化USART3 DMA接收陀螺仪数据
	HAL_UARTEx_ReceiveToIdle_DMA(&huart3, USART3_DMA_Buffer, sizeof(USART3_DMA_Buffer));
	__HAL_DMA_DISABLE_IT(&hdma_usart3_rx, DMA_IT_HT);

	// 初始化USART1 DMA接收陀螺仪数据
	HAL_UARTEx_ReceiveToIdle_DMA(&huart1, USART1_RxData, sizeof(USART1_RxData));
	__HAL_DMA_DISABLE_IT(&hdma_usart1_rx, DMA_IT_HT);

	HAL_UARTEx_ReceiveToIdle_DMA(&huart2, USART2_RxData, sizeof(USART2_RxData));
	__HAL_DMA_DISABLE_IT(&hdma_usart2_rx, DMA_IT_HT);

	drone_data.drone_x = 0.0f;
	drone_data.drone_y = 0.0f;
	drone_data.fire_id = 0; // 初始化无人机数据

	// 保留中断方式接收作为备用
	// HAL_UART_Receive_IT(&huart3, &USART3_RxData, 1);

	// 初始化发送标志
	uart_tx_busy = 0;
}

// void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
// {
// 	if (huart == &huart3)
// 	{
// 		// HAL_GPIO_TogglePin(GPIOD, GPIO_PIN_1);
// 		// //HAL_UART_Transmit_IT(&huart3,&USART3_RxData,1);
// 		// get_jy61p(USART3_RxData);
// 		// HAL_UART_Receive_IT(&huart3, &USART3_RxData, 1);
// 	}
// }

void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
{
	if (huart == &huart1) // 飞机和单片机
	{
		// 接收中断只设置标志位，不做解析
		new_data_flag = 1;

		// 动物数据解析 - 支持两种格式
		int temp_species_count = 0;
		char temp_grid[6] = {0};
		uint8_t parse_success = 0;

		// 尝试解析网格格式：animal,41,A3,B1,@
		if (parse_animal_grid_data((char *)USART1_RxData, &temp_species_count, temp_grid))
		{
			// 网格格式解析成功，直接保存数据
			animal_data.species_count = temp_species_count;
			strcpy(animal_data.grid_position, temp_grid);
			parse_success = 1;
		}
		else
		{
			// 尝试解析像素坐标格式：animal,41,240.0,480.0,@
			float temp_x, temp_y;
			if (parse_animal_data((char *)USART1_RxData, &temp_species_count, &temp_x, &temp_y))
			{
				// 将像素坐标转换为网格坐标
				if (pixel_to_grid(temp_x, temp_y, temp_grid))
				{
					// 坐标在有效范围内，保存数据
					animal_data.species_count = temp_species_count;
					strcpy(animal_data.grid_position, temp_grid);
					parse_success = 1;
				}
				// 如果坐标超出范围，直接抛弃（不做任何处理）
			}
		}

		// 发送解析结果
		if (parse_success)
		{
			char cmd[64];
			sprintf(cmd, "Animal: count=%d, pos=%s\r\n", temp_species_count, temp_grid);
			HAL_UART_Transmit_DMA(&huart1, (uint8_t *)cmd, strlen(cmd));
		}

		// 重新启动DMA接收
		HAL_UARTEx_ReceiveToIdle_DMA(&huart1, USART1_RxData, sizeof(USART1_RxData));
		__HAL_DMA_DISABLE_IT(&hdma_usart1_rx, DMA_IT_HT);
	}
	if (huart == &huart2) // 串口屏
	{

		// HAL_UART_Transmit_DMA(&huart2, (uint8_t*)lie, strlen(lie));
		// HAL_UART_Transmit_DMA(&huart2, USART2_RxData, Size);
		if (sscanf((char *)USART2_RxData, "fob%2s,%2s#", lie, hang) == 2)
		{
			// 转换坐标
			ConvertCoord(lie, hang, &row, &col);
			// SetVerticalNoFlyZone(row, col);
			SetNoFlyZone(row, col);

			extern uint8_t mode;

			mode = AnalyzeNoFlyZoneOrientation(row, col);

			// 添加到简单禁飞区数组
			AddNoFlyZone(row, col);

			dis_counter++;
		}
		// 重新启动DMA接收
		HAL_UARTEx_ReceiveToIdle_DMA(&huart2, USART2_RxData, sizeof(USART2_RxData));
		__HAL_DMA_DISABLE_IT(&hdma_usart2_rx, DMA_IT_HT);
	}

	if (huart == &huart3) // 陀螺仪
	{
		// 处理陀螺仪DMA数据
		HAL_GPIO_TogglePin(GPIOD, GPIO_PIN_1); // 指示灯切换状态
		get_jy61p_dma(USART3_DMA_Buffer, Size);

		// 重新启动DMA接收
		HAL_UARTEx_ReceiveToIdle_DMA(&huart3, USART3_DMA_Buffer, sizeof(USART3_DMA_Buffer));
		__HAL_DMA_DISABLE_IT(&hdma_usart3_rx, DMA_IT_HT);
	}
	else if (huart == &huart4) // 蓝牙通信
	{
		// HAL_UART_Transmit_DMA(&huart4, USART4_RxData, Size);
		// 在这里写火源坐标解析 调用 Mission_StartByFireId 解析任务
		//  解析数据
		sscanf((char *)USART4_RxData, "!,%f,%f,%f,%f,%f,#", &left_wheel_pid.kp, &left_wheel_pid.ki, &left_wheel_pid.kd, &g_left_target_speed, &g_right_target_speed);

		HAL_UARTEx_ReceiveToIdle_DMA(&huart4, USART4_RxData, sizeof(USART4_RxData));
		__HAL_DMA_DISABLE_IT(&hdma_uart4_rx, DMA_IT_HT);
	}
	else if (huart == &huart5) // 视觉通信
	{
		// OLED_ShowNum(10,6,11,2,16,0);

		sscanf((char *)USART5_RxData, "!,%f,%f,%d,#", &vision_data.error_x, &vision_data.error_y, &vision_data.target_detected);
		// 处理maixcam数据
		HAL_UARTEx_ReceiveToIdle_DMA(&huart5, USART5_RxData, sizeof(USART5_RxData));
		__HAL_DMA_DISABLE_IT(&hdma_uart5_rx, DMA_IT_HT);
	}
}

/**
 * @brief DMA发送完成回调函数
 */
void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
{
	if (huart == &huart4)
	{
		// 标记发送完成
		uart_tx_busy = 0;
	}
}

int fputc(int ch, FILE *f)
{
	// 采用轮询方式发送1字节数据，超时时间设置为无限等待
	HAL_UART_Transmit(&huart4, (uint8_t *)&ch, 1, HAL_MAX_DELAY);
	return ch;
}

// /**
//  * @brief 发送数据到VOFA+ (FireWater协议)
//  */
// void send_firewater_data(void)
// {
//     static uint32_t last_time = 0;
//     uint32_t now = HAL_GetTick();
//     if (now - last_time < 50) return; // 20Hz发送频率
//     last_time = now;

//     // 如果上一次发送未完成，则跳过本次发送
//     if (uart_tx_busy)
//         return;

//     // 准备数据
//     float data[4] = {left_wheel_speed, right_wheel_speed, IMU_data.YawZ, left_wheel_pid.out};

//     // FireWater协议的结束标志 0x00 0x00 0x80 0x7F
//     uint8_t tail[4] = {0x00, 0x00, 0x80, 0x7F};

//     // 将数据拷贝到发送缓冲区
//     memcpy(uart_tx_buffer, data, sizeof(data));
//     memcpy(uart_tx_buffer + sizeof(data), tail, sizeof(tail));

//     // 标记发送忙
//     uart_tx_busy = 1;

//     // 使用DMA发送数据
//     HAL_UART_Transmit_DMA(&huart4, uart_tx_buffer, sizeof(data) + sizeof(tail));
// }

/**
 * @brief 处理飞机数据（在主循环中每500ms调用一次）
 */
void Process_Drone_Data(void)
{
	static uint32_t last_process_time = 0;
	uint32_t current_time = HAL_GetTick();

	// 每500ms处理一次，且有新数据时才处理
	if ((current_time - last_process_time) >= 500 && new_data_flag)
	{
		// 验证数据包完整性（简单检查是否包含@结束符）
		if (strstr((char *)USART1_RxData, "@") != NULL)
		{
			// 解析code数据
			sscanf((char *)USART1_RxData, "code,%f,%f,@", &drone_data.drone_x, &drone_data.drone_y);
		}

		new_data_flag = 0; // 清除标志位
		last_process_time = current_time;
	}
}
